#include <stdio.h>
#include <stdlib.h>
#include <signal.h>
#include <unistd.h>
#include <sys/mman.h>
#include <string.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <netdb.h>
#include <native/task.h>
#include <native/timer.h>
#include <native/sem.h>
#include <native/mutex.h>
#include <native/cond.h>
#include <rtdk.h>
#include <time.h>
#include "msgProtocol.h"

//communications thread
RT_TASK communicationsThread;
RT_TASK statusThread, floorRun_task, supervisor_task;
RT_SEM stop;
//control access to status double buffers
RT_MUTEX statusMutex;
RT_MUTEX status_M, dropQueue_M, pickupQueue_M, statusBuffer_M[2];

void elevatorStop();

//communications thread unpacks any received data into this buffer
char comm_buffer[256];

//double buffer shared between status and communications threads
char status_read_buffer[256];
char status_write_buffer[256];

//queues for recording destinations
int * dropQueue;  //includes one set of floors
int * pickupQueue;  //includes set of floors for both directions

//tcp communications variables
int sockfd, portno;
struct sockaddr_in serv_addr;
struct hostent *server;

//elevator status variables
int id; //elevatorNumber
int state = 1; //elevatorState, states: moving(0), stopped(1), door opening(2), door open(3), door closing(4)
int currentFloor = 1;
int nextDest = 1; //nextFloor;
int direction = 1; // up (1), down(-1), idle(0)
int nextDir = 0;
int destinationCount = 1;
int dispatcherFail = 0;
int floorRunVar = 0;
//building variables - store number of floors and
//number of elevators in the building in case connection
//with dispatcher is disrupted or lost
int numberOfFloors = 10; //NumberOfFloors
int numberOfElevators = 1; //NumberOfElevators
int systemSpeed = 1;

//sleeps for various threads
RTIME statusUpdateSleep = 75000000L;

void printPickUpQueue(){
	int i;
	for(i = 0; i<2*numberOfFloors;i++){
		rt_printf("%d-%d\n", i, pickupQueue[i]);
	}
}

//setters and getters with mutexs for global variables
void setDropQueue(int index, int value){
	rt_mutex_acquire(&dropQueue_M,TM_INFINITE);
	dropQueue[index] = value;
	rt_mutex_release(&dropQueue_M);

}
int readDropQueue(int index){
	int tmp;
	rt_mutex_acquire(&dropQueue_M,TM_INFINITE);
	tmp = dropQueue[index];
	rt_mutex_release(&dropQueue_M);
	return tmp;
}
void setpickupQueue(int index, int value){
	rt_mutex_acquire(&pickupQueue_M,TM_INFINITE);
	pickupQueue[index] = value;
	rt_mutex_release(&pickupQueue_M);
}
int readpickupQueue(int index){
	int tmp;
	rt_mutex_acquire(&pickupQueue_M,TM_INFINITE);
	tmp = pickupQueue[index];
	rt_mutex_release(&pickupQueue_M);
	return tmp;
}
void updateVariable(int *var, int value, char *varName){
	rt_mutex_acquire(&status_M,TM_INFINITE);
	//rt_printf("Set %s = %d\n",varName, value);
	*var = value;
	rt_mutex_release(&status_M);
}
int readVariable(int *var){
	int temp;
	rt_mutex_acquire(&status_M,TM_INFINITE);
	temp = *var;
	rt_mutex_release(&status_M);
	return temp;
}
void removeFromQueue(int dir, int floor){
	rt_printf("Remove: %d/%d\n", dir, floor);
	//remove from drop Queue
	setDropQueue(floor-1, 0);
	//remove from pick up Queue
	if(dir == 1){
		setpickupQueue(floor-1, 0);
	}else{
		setpickupQueue(2*numberOfFloors-floor, 0);
	}
}

//pick up and drop off requests
void dropRequest(int floor, int dir){
	int CF = readVariable(&currentFloor);
	rt_printf("dropRequest: %d, %d\n", dir, floor);
	if(floor !=CF){
		if(CF*dir < floor*dir){
			setDropQueue(floor-1, 1);
		}
		if(dir == 0){
			if(CF>floor){
				updateVariable(&direction,-1,"Direction");
			}else if(CF<floor){
				updateVariable(&direction,1,"Direction");
			}
			setDropQueue(floor-1,1);
		}
		if(readVariable(&state) == 1){
			rt_printf("Signaled Floor Run\n");
			updateVariable(&state, 1, "elevatorState");
			rt_sem_v(&stop);
		}
	}
}
void pickUpRequest(int dir, int floor){
	if(floor !=currentFloor){
		rt_printf("PickupRequest: %d, %d\n", dir, floor);
		if(dir == 1){
			setpickupQueue(floor-1, 1);
		}else{
			setpickupQueue((2*numberOfFloors)-floor, 1);
		}
		if(readVariable(&state) == 1){
			rt_printf("Signaled Floor Run");
			updateVariable(&state, 1, "elevatorState");
			rt_sem_v(&stop);
		}
	}else{
		if(readVariable(&state)==1){
			elevatorStop();
		}
	}
}

/*----------Communications Thread functions----------*/
//send message to dispatcher over TCP
void sendMessage(char *msg) {
	sprintf(comm_buffer, msg);
	int ret = write(sockfd, comm_buffer, strlen(comm_buffer));
	if (ret < 0) {
		rt_printf("Error writing to socket\n");
	}
	bzero(comm_buffer, 256);
}

//receive message from dispatcher over TCP
void receiveMessage() {
	bzero(comm_buffer, 256);
	int ret = read(sockfd, comm_buffer, 255);
	if (ret < 0) {
		rt_printf("Error reading from socket\n");
	}
}

//get the type of message sent by the dispatcher
void processMessage() {
	char msgType = comm_buffer[0];
	if (msgType == REGISTRATION_COMPLETE) {
		//
		rt_printf("Registered with dispatcher\n");
	} else if (msgType == REQUESTUPDATE) {
		//rt_printf("Dispatcher requesting update\n");
		//send update to dispatcher
		bzero(comm_buffer, 256);
		rt_mutex_acquire(&statusMutex, TM_INFINITE);
		sendMessage(status_write_buffer);
		bzero(status_write_buffer, 256);
		rt_mutex_release(&statusMutex);
	} else if (msgType == HALLCALL) {
		int hc, floor, dir;
		sscanf(comm_buffer, "%d/%d/%d/*", &hc, &floor, &dir);

		rt_printf("%d\n", dir);

		pickUpRequest(dir, floor);
		char requestDir[4] = "\0";
		if (dir == 1) {
			sprintf(requestDir, "Up");
		} else {
			sprintf(requestDir, "Down");
		}
		rt_printf("Received hall call: Floor %d to go %s\n", floor, requestDir);
		bzero(comm_buffer, 256);
	} else if (msgType == ELEVATORCALL) {
		int ec, floor;
		sscanf(comm_buffer, "%d/%d/*", &ec, &floor);

		dropRequest(floor, readVariable(&direction));
		rt_printf("Received drop off request: Floor %d\n", floor);
		bzero(comm_buffer, 256);
	} else {
		rt_printf("Dispatcher Communications Failed\n");
		bzero(comm_buffer, 256);
		dispatcherFail = 1;
	}
}

//communications thread - all communication with dispatcher
void communications(void *arg) {
	//register with the dispatcher
	char initMsg[256];
	init(initMsg, id);
	sendMessage(initMsg);

	while (1) {
		if(!dispatcherFail){
			receiveMessage();
			processMessage();
		}
		else {
			return;
		}
	}

}

//setup a TCP connection with the dispatcher
int setupCommunications(int argc, char *argv[]) {
	//must have 3 arguments - ElevatorController <elevator_number> <server_name> <server_port>
	if (argc < 4) {
		rt_printf("usage %s hostname port\n", argv[0]);
		return -1;
	}

	//get elevator number
	id = atoi(argv[1]);

	//get the port number
	portno = atoi(argv[3]);

	//get the server host name
	server = gethostbyname(argv[2]);
	if (server == NULL) {
		rt_printf("Error, no such host\n");
		return -1;
	}

	//create the socket
	sockfd = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP);
	if (sockfd < 0) {
		rt_printf("Error opening socket\n");
		return -1;
	}

	//setup the socket
	bzero((char *) &serv_addr, sizeof(serv_addr));
	serv_addr.sin_family = AF_INET;
	bcopy((char *) server->h_addr, (char *) &serv_addr.sin_addr.s_addr,
			server->h_length);
	serv_addr.sin_port = htons(portno);
	if (connect(sockfd, (struct sockaddr *) &serv_addr, sizeof(serv_addr)) < 0) {
		rt_printf("Error connecting\n");
		return -1;
	}
	//communication established - setup successful
	return 0;
}

/*----------End of Communications Thread functions----------*/

/*----------Status Thread functions----------*/

//get current status and update status buffer
//status buffer sent to dispatcher every 75ms by
//communications thread
int getNextFloor(dir){
	int i;
	int CF = readVariable(&currentFloor);
	int NOF = readVariable(&numberOfFloors);
	if(dir == 1){
		//next floor in current direction
		for(i=CF; i<NOF;i++){
			if((readDropQueue(i) ==1) | (readpickupQueue(i) == 1)){
				floorRunVar = 1;
				updateVariable(&nextDir, 1, "nextDir");
				return i+1;
			}
		}
		//next floor in other direction
		for(i=NOF; i<2*NOF; i++){
			if(readpickupQueue(i) == 1){
				floorRunVar = 2;
				updateVariable(&nextDir, -1, "nextDir");
				return 2*NOF - i;
			}
		}
		for(i=0; i<CF; i++){
			if(readpickupQueue(i) == 1){
				floorRunVar = 3;
				updateVariable(&nextDir, 1, "nextDir");
				return i+1;
			}
		}
	}else{
		//next floor in current direction
		for(i=CF; i>0;i--){
			if((readDropQueue(i-1) ==1) | (readpickupQueue(2*NOF-i) == 1)){
				floorRunVar = 4;
				updateVariable(&nextDir, -1, "nextDir");
				return i;
			}
		}
		//next floor in other direction
		for(i=0; i<NOF; i++){
			if(readpickupQueue(i) == 1){
				floorRunVar = 5;
				updateVariable(&nextDir, 1, "nextDir");
				return i+1;
			}
		}
		for(i=NOF; i<2*NOF-CF; i++){
			if(readpickupQueue(i) == 1){
				floorRunVar = 6;
				updateVariable(&nextDir, -1, "nextDir");
				return (2*NOF - i);
			}
		}
	}

	return CF;
}

void status(void *arg) {
	char updateMsg[256];
	rt_task_set_periodic(NULL, TM_NOW, statusUpdateSleep);
	while (1) {
		rt_mutex_acquire(&statusMutex, TM_INFINITE);
		update(updateMsg, id, state, currentFloor, nextDest, direction,
				destinationCount);
		bcopy(updateMsg, status_write_buffer, 256);
		rt_mutex_release(&statusMutex);
		updateVariable(&nextDest, getNextFloor(direction),"Next Destination");
		rt_task_wait_period(NULL);
	}
}

/*----------End of Status Thread functions----------*/

/*----------Floor Run Thread functions----------*/
void elevatorStop(){

	//elevator stops
	updateVariable(&direction, readVariable(&nextDir),"Direction");
	updateVariable(&state, 1, "elevatorState");
	removeFromQueue(direction, currentFloor);
	//doors opening
	updateVariable(&state, 2, "elevatorState");
	rt_task_sleep(1500000000L);
	//doors open
	updateVariable(&state, 3, "elevatorState");
	rt_task_sleep(2000000000L);
	//doors closing
	updateVariable(&state, 4, "elevatorState");
	rt_task_sleep(1500000000L);
	updateVariable(&state, 1, "elevatorState");
	rt_printf("Direction: %d\n", direction);
}
void elevatorRun(int dir){
	updateVariable(&state, 0, "elevatorState");
	rt_task_set_periodic(NULL, TM_NOW, 2000000000L);
	while(1){
		rt_task_wait_period(NULL);
		updateVariable(&currentFloor, readVariable(&currentFloor) + dir, "Current Floor");
		if(readVariable(&currentFloor)>numberOfFloors){
			updateVariable(&currentFloor, readVariable(&numberOfFloors), "Current Floor");
			rt_printf("Error Reseting\n");
			updateVariable(&direction, -1, "direction");
			elevatorStop();
			pickUpRequest(1,1);
			break;
		}
		if(readVariable(&currentFloor)<1){
			updateVariable(&currentFloor, 1, "Current Floor");
			rt_printf("Error Reseting\n");
			updateVariable(&direction, 1, "direction");
			elevatorStop();
			pickUpRequest(1,1);
			break;
		}
		rt_printf("C: %d, N: %d, D: %d FRV: %d\n", currentFloor, nextDest, dir, floorRunVar);
		if(readVariable(&currentFloor) == readVariable(&nextDest)){
			rt_printf("STOPPING\n");
			elevatorStop();
			break;
		}
	}
}

//floorrun Thread
void floorRun(void *arg){
	int CF;
	int NF;
	while(1){
		updateVariable(&nextDest, getNextFloor(readVariable(&direction)),"Next Destination");
		CF = readVariable(&currentFloor);
		NF = readVariable(&nextDest);
		rt_printf("CF= %d, NF= %d.\n", CF, NF);
		if(CF > NF){
			rt_printf("Going Down\n");
			elevatorRun(-1);
		}else if(CF < NF) {
			rt_printf("Going Up\n");
			elevatorRun(1);
		}else{
			rt_printf("Stopped\n");
			updateVariable(&direction,0, "Direction");
			//wait for signal
			rt_sem_p(&stop,TM_INFINITE);
			//sleep(2000);
			//break;
		}
	}
}
/*----------End of Floor Run Thread functions----------*/

/*----------Start of Supervisor Thread functions----------*/
//supervisor Thread
void supervisor(void *arg){
	//wait for a signal
	//add requests to pickup queue
	rt_task_set_periodic(NULL, TM_NOW, 1000000000L);
	while(1){
		if(dispatcherFail){

			int i;
			rt_printf("Supervisor Activated\n");
			for(i = numberOfFloors;i>1;i = i--){
				pickUpRequest(-1,i);
				rt_task_wait_period(NULL);
			}
			pickUpRequest(1,1);
			while(currentFloor == 1){
				rt_task_wait_period(NULL);
			}
			while(currentFloor != 1){
				rt_task_wait_period(NULL);
			}
		}
		rt_task_wait_period(NULL);
	}
}
/*----------End of Supervisor Thread functions----------*/

void catch_signal(int sig) {
}

int main(int argc, char *argv[]) {
	mlockall(MCL_CURRENT | MCL_FUTURE);
	signal(SIGTERM, catch_signal);
	signal(SIGINT, catch_signal);
	rt_print_auto_init(1);

	int ret = setupCommunications(argc, argv);
	if (ret == -1) {
		rt_printf("Error setting up communications with dispatcher\n");
		return -1;
	}
	pickupQueue = realloc(pickupQueue, numberOfFloors*2*sizeof(int));
	dropQueue = realloc(dropQueue, numberOfFloors*sizeof(int));


	rt_sem_create(&stop,NULL,0,S_FIFO);
	rt_mutex_create(&status_M,NULL);
	rt_mutex_create(&dropQueue_M,NULL);
	rt_mutex_create(&pickupQueue_M,NULL);
	rt_mutex_create(&statusMutex, NULL);

	rt_task_create(&communicationsThread, NULL, 0, 99, T_JOINABLE);
	rt_task_create(&statusThread, NULL, 1, 99, T_JOINABLE);
	rt_task_create(&floorRun_task, NULL, 0, 99, T_JOINABLE);
	rt_task_create(&supervisor_task, NULL, 0, 99, T_JOINABLE);

	rt_task_start(&communicationsThread, &communications, NULL);
	rt_task_start(&statusThread, &status, NULL);
	rt_task_start(&floorRun_task, &floorRun, NULL);
	rt_task_start(&supervisor_task, &supervisor, NULL);

	rt_task_join(&floorRun_task);
	rt_task_join(&communicationsThread);
	rt_task_join(&statusThread);
	rt_task_join(&supervisor_task);

	rt_mutex_delete(&statusMutex);
	rt_mutex_delete(&status_M);
	rt_mutex_delete(&dropQueue_M);
	rt_mutex_delete(&pickupQueue_M);

	rt_task_delete(&communicationsThread);
	rt_task_delete(&statusThread);
	rt_task_delete(&floorRun_task);
	rt_task_delete(&supervisor_task);
	rt_printf("Elevator Controller shutting down\n");

	return 0;
}
